#include "rads_database.hpp"
std::mutex g_db_mutex;
// 接受网络传来指令存库
void pradscmdCallback(const rads::rads_cmd::ConstPtr &msg)
{
    tcp_cmd cmd;
    cmd.type = 1;
    cmd.cmd = msg->pro_nema;
    cmd.ctime = getCurrentTimeString();
    if (insert_cmd(cmd))
    {
        //  ROS_INFO("cmd saved successfully");
    }
    else
    {
        ROS_ERROR("Failed to save cmd");
    }
}
// 接受传感器数据（rtk+激光雷达数据）
void pradsfusionCallback(const rads::rads_trackext::ConstPtr &msg)
{
    // 更新最新数据
    latest_car_log_data.speed = isnan(msg->groud_speed) ? 0.0 : msg->groud_speed;
    latest_car_log_data.x = isnan(msg->x) ? 0.0 : msg->x;
    latest_car_log_data.y = isnan(msg->y) ? 0.0 : msg->y;
    latest_car_log_data.z = isnan(msg->z) ? 0.0 : msg->z;
    latest_car_log_data.lidar_heading = isnan(quaternionToEulerAngle(msg->ox, msg->oy, msg->oz, msg->ow)) ? 0.0 : quaternionToEulerAngle(msg->ox, msg->oy, msg->oz, msg->ow);
    latest_car_log_data.ndt_matching = isnan(msg->matching_rate) ? 0.0 : msg->matching_rate;
    latest_car_log_data.rtk_x = isnan(msg->rtk_x) ? 0.0 : msg->rtk_x;
    latest_car_log_data.rtk_y = isnan(msg->rtk_y) ? 0.0 : msg->rtk_y;
    latest_car_log_data.rtk_z = isnan(msg->rtk_z) ? 0.0 : msg->rtk_z;
    latest_car_log_data.lng = isnan(msg->longitude) ? 0.0 : msg->longitude;
    latest_car_log_data.lat = isnan(msg->latitude) ? 0.0 : msg->latitude;
    latest_car_log_data.rtk_heading = isnan(msg->heading) ? 0.0 : msg->heading;
    latest_car_log_data.position_flag = isnan(msg->posi_flag) ? 0.0 : msg->posi_flag;
    latest_car_log_data.orientation_flag = isnan(msg->head_flag) ? 0.0 : msg->head_flag;
    
    // 标记数据已更新
    car_log_updated = true;
}
// sradsack话题功能：1.
void sradsackCallback(const rads::rads_acks::ConstPtr &msg)
{

    // 设置开机点
    if (msg->cmd == 16 && msg->ack == 1 && msg->ext2 == 1)
    {
        start_machine start_machine_data;
        start_machine_data.map_id = msg->map_id;
        if (!msg->trajectory.poses.empty())
        {
            start_machine_data.x = msg->trajectory.poses[0].pose.position.x;
            start_machine_data.y = msg->trajectory.poses[0].pose.position.y;
            start_machine_data.z = msg->trajectory.poses[0].pose.position.z;
            start_machine_data.ox = msg->trajectory.poses[0].pose.orientation.x;
            start_machine_data.oy = msg->trajectory.poses[0].pose.orientation.y;
            start_machine_data.oz = msg->trajectory.poses[0].pose.orientation.z;
            start_machine_data.ow = msg->trajectory.poses[0].pose.orientation.w;
        }
        if (!msg->trajectory_ext.empty())
        {
            start_machine_data.num = msg->trajectory_ext[0].point_count;
            start_machine_data.ndt_matching = msg->trajectory_ext[0].matching_rate;
            start_machine_data.rtk_x = msg->trajectory_ext[0].rtk_x;
            start_machine_data.rtk_y = msg->trajectory_ext[0].rtk_y;
            start_machine_data.rtk_z = msg->trajectory_ext[0].rtk_z;
            start_machine_data.lng = msg->trajectory_ext[0].longitude;
            start_machine_data.lat = msg->trajectory_ext[0].latitude;
            start_machine_data.rtk_heading = msg->trajectory_ext[0].heading;
            start_machine_data.position_flag = msg->trajectory_ext[0].posi_flag;
            start_machine_data.orientation_flag = msg->trajectory_ext[0].head_flag;
            start_machine_data.lidar_heading = msg->trajectory_ext[0].ld_heading;
        }
        if (insert_start_machine(start_machine_data))
        {
            ROS_INFO("start_machine saved successfully");
        }
        else
        {
            ROS_ERROR("Failed to save start_machine");
        }
    }
    // 删除开机点
    if (msg->cmd == 16 && msg->ack == 1 && msg->ext2 == 0)
    {
        start_machine start_machine_data;
        start_machine_data.map_id = msg->map_id;
        start_machine_data.num = msg->ext1;
        if (delete_start_machine(start_machine_data))
        {
            ROS_INFO("delete_start_machine saved successfully");
        }
        else
        {
            ROS_ERROR("Failed to save delete_start_machine");
        }
    }
    // 创建路线
    if (msg->cmd == 12 && msg->ack == 2 && msg->ext2 == 1)
    {
        car car_data;
        car_data.map_id = msg->map_id;
        car_data.line_id = msg->path_id;
        if (!msg->trajectory_ext.empty())
        {
            car_data.x = msg->trajectory_ext[0].x;
            car_data.y = msg->trajectory_ext[0].y;
            car_data.z = msg->trajectory_ext[0].z;
            car_data.lidar_heading = msg->trajectory_ext[0].ld_heading;
            car_data.ndt_matching = msg->trajectory_ext[0].matching_rate;
            car_data.rtk_x = msg->trajectory_ext[0].rtk_x;
            car_data.rtk_y = msg->trajectory_ext[0].rtk_y;
            car_data.rtk_z = msg->trajectory_ext[0].rtk_z;
            car_data.rtk_heading = msg->trajectory_ext[0].heading;
            car_data.lng = msg->trajectory_ext[0].longitude;
            car_data.lat = msg->trajectory_ext[0].latitude;
            car_data.position_flag = msg->trajectory_ext[0].posi_flag;
            car_data.orientation_flag = msg->trajectory_ext[0].head_flag;
            car_data.num_point = msg->trajectory_ext[0].point_count;
            car_data.ext =msg->trajectory_ext[0].ext;
            car_data.stop_flag = 0;
            car_data.stop_time = 0;
        }
        if (insert_car(car_data))
        {
            ROS_INFO("car saved successfully (num_point:%d)", car_data.num_point);
        }
        else
        {
            ROS_ERROR("Failed to save car");
        }
    }
       // 创建路线B
    if (msg->cmd == 1024 && msg->ack == 2 && msg->ext2 == 1)
    {
        car car_data;
        car_data.map_id = msg->map_id;
        car_data.line_id = msg->path_id;
        if (!msg->trajectory_ext.empty())
        {
            car_data.x = msg->trajectory_ext[0].x;
            car_data.y = msg->trajectory_ext[0].y;
            car_data.z = msg->trajectory_ext[0].z;
            car_data.lidar_heading = msg->trajectory_ext[0].ld_heading;
            car_data.ndt_matching = msg->trajectory_ext[0].matching_rate;
            car_data.rtk_x = msg->trajectory_ext[0].rtk_x;
            car_data.rtk_y = msg->trajectory_ext[0].rtk_y;
            car_data.rtk_z = msg->trajectory_ext[0].rtk_z;
            car_data.rtk_heading = msg->trajectory_ext[0].heading;
            car_data.lng = msg->trajectory_ext[0].longitude;
            car_data.lat = msg->trajectory_ext[0].latitude;
            car_data.position_flag = msg->trajectory_ext[0].posi_flag;
            car_data.orientation_flag = msg->trajectory_ext[0].head_flag;
            car_data.num_point = msg->trajectory_ext[0].point_count;
            car_data.stop_flag = 0;
            car_data.stop_time = 0;
            car_data.ext =msg->trajectory_ext[0].ext;
        }
        if (insert_carB(car_data))
        {
            ROS_INFO("car saved successfully (num_point:%d)", car_data.num_point);
        }
        else
        {
            ROS_ERROR("Failed to save car");
        }
    }
    // 马踏飞燕
    if (msg->cmd == 888)
    {
        point_all point_all_data;
        point_all_data.map_id = msg->map_id;
        if (!msg->trajectory_ext.empty())
        {
            point_all_data.x = msg->trajectory_ext[0].x;
            point_all_data.y = msg->trajectory_ext[0].y;
            point_all_data.z = msg->trajectory_ext[0].z;
            point_all_data.lidar_heading = msg->trajectory_ext[0].ld_heading;
            point_all_data.ndt_matching = msg->trajectory_ext[0].matching_rate;
            point_all_data.rtk_x = msg->trajectory_ext[0].rtk_x;
            point_all_data.rtk_y = msg->trajectory_ext[0].rtk_y;
            point_all_data.rtk_z = msg->trajectory_ext[0].rtk_z;
            point_all_data.lng = msg->trajectory_ext[0].longitude;
            point_all_data.lat = msg->trajectory_ext[0].latitude;
            point_all_data.rtk_heading = msg->trajectory_ext[0].heading;
            point_all_data.position_flag = msg->trajectory_ext[0].posi_flag;
            point_all_data.orientation_flag = msg->trajectory_ext[0].head_flag;
        }
        if (insert_point_all(point_all_data))
        {
            ROS_INFO("point_all saved successfully");
        }
        else
        {
            ROS_ERROR("Failed to save point_all");
        }
    }
    // goback踩点
    if (msg->cmd == 999)
    {
        go_back go_back_data;
        if (!msg->trajectory_ext.empty())
        {
            go_back_data.num_point = msg->trajectory_ext[0].point_count;
            go_back_data.x = msg->trajectory_ext[0].x;
            go_back_data.y = msg->trajectory_ext[0].y;
            go_back_data.z = msg->trajectory_ext[0].z;
            go_back_data.lidar_heading = msg->trajectory_ext[0].ld_heading;
            go_back_data.rtk_x = msg->trajectory_ext[0].rtk_x;
            go_back_data.rtk_y = msg->trajectory_ext[0].rtk_y;
            go_back_data.rtk_z = msg->trajectory_ext[0].rtk_z;
            go_back_data.rtk_heading = msg->trajectory_ext[0].heading;
            go_back_data.lng = msg->trajectory_ext[0].longitude;
            go_back_data.lat = msg->trajectory_ext[0].latitude;
            go_back_data.position_flag = msg->trajectory_ext[0].posi_flag;
            go_back_data.orientation_flag = msg->trajectory_ext[0].head_flag;
        }
        if (insert_go_back(go_back_data))
        {
            ROS_INFO("go_back saved successfully");
        }
        else
        {
            ROS_ERROR("Failed to save go_back");
        }
    }
    // 保存任意点路线点
    if (msg->cmd == 30 && msg->ack == 2 && msg->ext2 != 0)
    {
        delete_point_data();
        point_data point_data_data;
        bool all_success = true;
        for (const auto &ext : msg->trajectory_ext)
        {
            point_data_data.x = ext.x;
            point_data_data.y = ext.y;
            point_data_data.z = ext.z;
            point_data_data.lidar_heading = ext.ld_heading;
            point_data_data.ndt_matching = ext.matching_rate;
            point_data_data.rtk_x = ext.rtk_x;
            point_data_data.rtk_y = ext.rtk_y;
            point_data_data.rtk_z = ext.rtk_z;
            point_data_data.lng = ext.longitude;
            point_data_data.lat = ext.latitude;
            point_data_data.rtk_heading = ext.heading;
            point_data_data.position_flag = ext.posi_flag;
            point_data_data.orientation_flag = ext.head_flag;
            point_data_data.num_point = ext.point_count;
            point_data_data.alltotal = ext.alltotal;
            // 存入数据库
            if (!insert_point_data(point_data_data))
            {
                ROS_ERROR("Failed to save point_data point (count: %d)", ext.point_count);
                all_success = false;
            }
        }

        if (all_success)
        {
            ROS_INFO("All %zu trajectory point_data saved successfully", msg->trajectory_ext.size());
        }
        else
        {
            ROS_WARN("Some trajectory point_data failed to save");
        }
    }
    // 结束建图，保存地图编号到数据库
    if (msg->cmd == 10 && msg->ack == 1 && msg->ext2 == 0)
    {
        fist_pose fist_pose_data;
        fist_pose_data.map_id = msg->map_id;
        // 存入数据库
        if (insert_fist_pose(fist_pose_data))
        {
            ROS_INFO("fist_pose saved successfully");
        }
        else
        {
            ROS_ERROR("Failed to save fist_pose");
        }
    }
}
// 接受自动驾驶控车信息
void radsdrvingCallback(const rads::rads_driving::ConstPtr &msg)
{
    car_driving_log car_driving_log_data;
    car_driving_log_data.model = isnan(msg->model) ? 0.0 : msg->model;
    car_driving_log_data.steering = isnan(msg->steering) ? 0.0 : msg->steering;
    car_driving_log_data.acc = isnan(msg->acc) ? 0.0 : msg->acc;
    car_driving_log_data.brake = isnan(msg->brake) ? 0.0 : msg->brake;
    car_driving_log_data.gear = isnan(msg->gear) ? 0.0 : msg->gear;
    car_driving_log_data.target_speed = isnan(msg->target_speed) ? 0.0 : msg->target_speed;
    car_driving_log_data.rpml = isnan(msg->rpml) ? 0.0 : msg->rpml;
    car_driving_log_data.rpmr = isnan(msg->rpmr) ? 0.0 : msg->rpmr;
    car_driving_log_data.lignt = isnan(msg->lignt) ? 0.0 : msg->lignt;
    car_driving_log_data.audio = isnan(msg->audio) ? 0.0 : msg->audio;
    car_driving_log_data.sign = isnan(msg->sign) ? 0.0 : msg->sign;
    ;
    car_driving_log_data.camera_hor = isnan(msg->camera_hor) ? 0.0 : msg->camera_hor;
    car_driving_log_data.camera_ver = isnan(msg->camera_ver) ? 0.0 : msg->camera_ver;
    car_driving_log_data.unknow_1 = isnan(msg->unknow_1) ? 0.0 : msg->unknow_1;
    car_driving_log_data.unknow_2 = isnan(msg->unknow_2) ? 0.0 : msg->unknow_2;
    car_driving_log_data.unknow_3 = isnan(msg->unknow_3) ? 0.0 : msg->unknow_3;
    car_driving_log_data.unknow_4 = isnan(msg->unknow_4) ? 0.0 : msg->unknow_4;
    car_driving_log_data.unknow_5 = isnan(msg->unknow_5) ? 0.0 : msg->unknow_5;
    car_driving_log_data.map_id = isnan(msg->map_id) ? 0.0 : msg->map_id;
    car_driving_log_data.line_id = isnan(msg->line_id) ? 0.0 : msg->line_id;
    car_driving_log_data.num_point = isnan(msg->num_point) ? 0.0 : msg->num_point;
    car_driving_log_data.alltotal = isnan(msg->alltotal) ? 0.0 : msg->alltotal;
    car_driving_log_data.ctime = getCurrentTimeString();
    if (insert_driving_log(car_driving_log_data))
    {
        // ROS_INFO("driving_log saved successfully");
    }
    else
    {
        ROS_ERROR("Failed to save driving_log");
    }
}

// 接受pcb命令数据
void pradsscmdCallback(const rads::rads_scmd::ConstPtr &msg)
{
    // 更新最新数据
    latest_pcb_log_data.model = isnan(msg->model) ? 0.0 : msg->model;
    latest_pcb_log_data.soc = isnan(msg->soc) ? 0.0 : msg->soc;
    latest_pcb_log_data.err = isnan(msg->err) ? 0.0 : msg->err;
    latest_pcb_log_data.rpm_l = isnan(msg->rpm_l) ? 0.0 : msg->rpm_l;
    latest_pcb_log_data.rpm_r = isnan(msg->rpm_r) ? 0.0 : msg->rpm_r;
    latest_pcb_log_data.key_1 = isnan(msg->key_1) ? 0.0 : msg->key_1;
    latest_pcb_log_data.key_2 = isnan(msg->key_2) ? 0.0 : msg->key_2;
    latest_pcb_log_data.key_3 = isnan(msg->key_3) ? 0.0 : msg->key_3;
    latest_pcb_log_data.key_4 = isnan(msg->key_4) ? 0.0 : msg->key_4;
    latest_pcb_log_data.key_5 = isnan(msg->key_5) ? 0.0 : msg->key_5;
    latest_pcb_log_data.key_6 = isnan(msg->key_6) ? 0.0 : msg->key_6;
    latest_pcb_log_data.key_7 = isnan(msg->key_7) ? 0.0 : msg->key_7;
    latest_pcb_log_data.key_8 = isnan(msg->key_8) ? 0.0 : msg->key_8;
    latest_pcb_log_data.ladar_1 = isnan(msg->ladar_1) ? 0.0 : msg->ladar_1;
    latest_pcb_log_data.ladar_2 = isnan(msg->ladar_2) ? 0.0 : msg->ladar_2;
    latest_pcb_log_data.ladar_3 = isnan(msg->ladar_3) ? 0.0 : msg->ladar_3;
    latest_pcb_log_data.ladar_4 = isnan(msg->ladar_4) ? 0.0 : msg->ladar_4;
    latest_pcb_log_data.ladar_5 = isnan(msg->ladar_5) ? 0.0 : msg->ladar_5;
    latest_pcb_log_data.ladar_6 = isnan(msg->ladar_6) ? 0.0 : msg->ladar_6;
    latest_pcb_log_data.ladar_7 = isnan(msg->ladar_7) ? 0.0 : msg->ladar_7;
    latest_pcb_log_data.ladar_8 = isnan(msg->ladar_8) ? 0.0 : msg->ladar_8;
    latest_pcb_log_data.unknow_1 = isnan(msg->unknow_1) ? 0.0 : msg->unknow_1;
    latest_pcb_log_data.unknow_2 = isnan(msg->unknow_2) ? 0.0 : msg->unknow_2;
    latest_pcb_log_data.unknow_3 = isnan(msg->unknow_3) ? 0.0 : msg->unknow_3;
    latest_pcb_log_data.unknow_4 = isnan(msg->unknow_4) ? 0.0 : msg->unknow_4;
    latest_pcb_log_data.unknow_5 = isnan(msg->unknow_5) ? 0.0 : msg->unknow_5;
    latest_pcb_log_data.unknow_6 = isnan(msg->unknow_6) ? 0.0 : msg->unknow_6;
    latest_pcb_log_data.heartbeat = isnan(msg->heartbeat) ? 0.0 : msg->heartbeat;
    
    // 标记数据已更新
    pcb_log_updated = true;
}


void pradsmessagecmdCallback(const rads::TaskMessage::ConstPtr &msg)
{
    car_cron car_cron_data;
    car_cron_data.ts = msg->ts;
    car_cron_data.uid = msg->uid;
    car_cron_data.opt = msg->opt;
    car_cron_data.vehicleCode = msg->vehicleCode;
    car_cron_data.executeType = msg->executeType;
    car_cron_data.cron = msg->cron;
    car_cron_data.taskCode = msg->taskCode;
    car_cron_data.num = msg->num;
    car_cron_data.data = msg->data;
    car_cron_data.addtime = getCurrentTimeString();

    // 解析data字段
    
    if(car_cron_data.opt == 3){
            //删除计划任务
            char command[512];
            snprintf(command, sizeof(command),
                    "cd /home/argo/catkin_ws/src/rads-master/shell && "
                    "./remove_cron_job.sh.sh");
            printf("删除计划任务: %s\n", command);
            int result = system(command);
    }else{
        parseDataString(car_cron_data.data, car_cron_data.dataItems);
        if (insert_car_cron_data(car_cron_data))
        {
            ROS_INFO("Car cron data saved successfully");
        }
        else
        {
            ROS_ERROR("Failed to save car cron data");
        }
    }
    
}








// 定时器回调函数，每3秒执行一次
void saveLogsTimerCallback(const ros::TimerEvent&)
{
    // 保存car_log数据
    if (car_log_updated)
    {
        latest_car_log_data.ctime = getCurrentTimeString();
        if (insert_car_log(latest_car_log_data))
        {
            // ROS_INFO("car_log saved successfully");
        }
        else
        {
            ROS_ERROR("Failed to save car_log");
        }
        car_log_updated = false;
    }
    
    // 保存pcb_log数据
    if (pcb_log_updated)
    {
        latest_pcb_log_data.ctime = getCurrentTimeString();
        if (insert_pcb_log(latest_pcb_log_data))
        {
            // ROS_INFO("pcb_log saved successfully");
        }
        else
        {
            ROS_ERROR("Failed to save pcb_log");
        }
        pcb_log_updated = false;
    }
}
int main(int argc, char **argv)
{
    ros::init(argc, argv, NODE_DATABASE);
    ros::NodeHandle nh;
    // 订阅网络发来指令(存库)
    static ros::Subscriber pradscmd = nh.subscribe("pradscmd", 1000, pradscmdCallback);
    // 订阅融合后的雷达+RTK数据（存库车辆实时的状态）
    static ros::Subscriber pradsfusion = nh.subscribe("pradsfusion", 1, pradsfusionCallback);
    // 订阅踩点信息（主要用于响应网络指令，上传地图，上传路线点等）
    static ros::Subscriber sradsack = nh.subscribe("sradsack", 1, sradsackCallback);
    // 订阅控制车辆的信息（存储控制车的信息）
    static ros::Subscriber radsdrving = nh.subscribe("radsdrving", 1, radsdrvingCallback);
    // 订阅下位机发来的指令 pradsscmd
    static ros::Subscriber pradsscmd = nh.subscribe("pradsscmd", 1, pradsscmdCallback);
    // 创建定时器，每3秒执行一次
    ros::Timer timer = nh.createTimer(ros::Duration(3.0), saveLogsTimerCallback);
    //订阅任务包话题 存库
    static ros::Subscriber pradsmessagecmdsub = nh.subscribe("pradsmessagecmd", 10, pradsmessagecmdCallback);

    ros::spin();
    return 0;
}